#include <iostream>
#include <unistd.h>

#include <opencv2/opencv.hpp>

#include <wiringPi.h>
#include <softPwm.h>
#include "simple_socket_protocol/Server.h"
#include "hal/CameraManager.h"

using ssp::ProtocolData;
using ssp::Server;

const uint64_t ONE_DAY_LONG_MS{(static_cast<uint64_t>(24 * 60 * 60) * 1000)};
const uint64_t SYS_CLOCK{24000000}; // Hz，默认时钟输入频率
const uint64_t PWM_CLOCK{100000};
const uint64_t PWM_PIN{21};                          // 设置输出 PWM 引脚为 wiP 21
const uint64_t PWM_CYCLE{20 * PWM_CLOCK / 1000};     // 设置 PMW 的周期为 20ms
const uint64_t PWM_DUTY_MAX{25 * PWM_CLOCK / 10000}; // 设置最大脉宽为 2.5ms，即占空比为 12.5%
const uint64_t PWM_DUTY_MIN{5 * PWM_CLOCK / 10000};  // 设置最大脉宽为 0.5ms，即占空比为 2.5%

Server server;
std::vector<std::shared_ptr<Camera>> cameras{};

std::vector<CameraFrame> capture(uint64_t delay_ms = 0)
{
    std::vector<CameraFrame> ret{};
    ret.reserve(cameras.size() * 20); // 一共拍20次
    int val = PWM_DUTY_MAX;           // 脉冲宽度变化量
    pwmWrite(PWM_PIN, val);
    delay(3000); // 等待舵机归位
    while (val > PWM_DUTY_MIN)
    {
        std::cout << "Taking photo" << std::endl;
        for (auto camera : cameras)
        {
            ret.emplace_back(camera->GetFrame());
        }
        val -= 10; // 设置变化脉宽为0.1ms，即一次转18°
        pwmWrite(PWM_PIN, val);
        delay(delay_ms);
    }
    return ret;
}

std::vector<ProtocolData> handleData(ProtocolData &data)
{
    std::vector<ProtocolData> ret{};
    if (data.getInfo() == "capture")
    {
        uint64_t ms_count{0};
        data.getExtraData(reinterpret_cast<uint8_t*>(&ms_count), sizeof(uint64_t));
        std::vector<CameraFrame> frames{capture(ms_count)};// recommended ms_count = 250
        uint32_t imcount{static_cast<uint32_t>(frames.size())};
        ret.emplace_back("imcount", reinterpret_cast<const uint8_t*>(&imcount), 4);
        for (cv::Mat &frame : frames)
        {
	    std::vector<uchar> compressed_frame{};
	    std::vector<int> params{cv::IMWRITE_PNG_COMPRESSION, 0};
	    cv::imencode(".png", frame, compressed_frame, params);
            data.setExtraData(compressed_frame.data(), compressed_frame.size());
            ret.push_back(data);
        }
    }
    else if (data.getInfo() == "stop")
    {
        std::cout << "Recv stop command" << std::endl;
        exit(0);
    }
    else
    {
        ret.push_back(data);
    }
    return ret;
}

int main()
{
    if (wiringPiSetup() == -1)
    {
        std::cout << "wiringPiSetup Failed!" << std::endl;
        exit(1);
    }
	pinMode(PWM_PIN, PWM_OUTPUT);
	pwmSetMode(PWM_PIN, PWM_MODE_MS);
	pwmSetClock(PWM_PIN, SYS_CLOCK / PWM_CLOCK); // 分频->100000Hz即0.01ms计一次数
	pwmWrite(PWM_PIN, 0); // 先将脉宽设为0，防止CCR大于ARR导致报错
	pwmSetRange(PWM_PIN, PWM_CYCLE);		// 周期为计数2000次，即20ms
    for (uint32_t i = 0; i < CameraManager::GetInstance()->GetCameraCount(); ++i)
    {
        std::shared_ptr<Camera> camera{CameraManager::GetInstance()->GetOrOpenCamera(i)};
        cameras.push_back(camera);
    }
    std::cout << "Waiting for connection..." << std::endl;
    while (!server.isConnectionEstablished())
    {
    }
    std::cout << "Connected!" << std::endl;
    while (true)
    {
        ProtocolData data{server.recv()};
        std::cout << "Recv: " << data.getInfo() << std::endl;
        std::vector<ProtocolData> retDataVec{handleData(data)};
        for (auto retData : retDataVec)
        {
            server.send(retData);
        }
        if (!server.isConnectionEstablished())
        {
            std::cout << "Disconnected, waiting for connection..." << std::endl;
            while (!server.isConnectionEstablished())
            {
            }
            std::cout << "Connected!" << std::endl;
        }
    }
    return 0;
}
